// motor.c

#include "motor.h"
#include "ecrt.h"
#include "igh.h"
#include "mqtt_comm.h"
#include "term.h"
#include <pthread.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>

// --- MODIFIED START ---
// 将比例系数从 10000 (mm) 修改为 10 (µm)
// 1 µm = 10 motor counts
static const double AXIS1_K = 10;

extern MotorSet cmdSet;
extern pthread_mutex_t cmdSet_mutex;

/**********************CStruct**********************************/
ec_pdo_entry_info_t slave_motor_pdo_entries[] = {
    {0x6040, 0x00, 16}, // 控制字 U16
    {0x6060, 0x00, 8},  // 操作模式 I8
    {0x607a, 0x00, 32}, // 目标位置 S32

    {0x6041, 0x00, 16}, // 状态字 U16
    {0x6061, 0x00, 8},  // 操作模式显示 I8
    {0x6064, 0x00, 32}, // 当前位置 S32
};

ec_pdo_info_t slave_motor_pdos[] = {
    {0x1600, 3, slave_motor_pdo_entries + 0}, // RXPDO
    {0x1a00, 3, slave_motor_pdo_entries + 3}, // TXPDO
};

ec_sync_info_t slave_motor_syncs[] = {
    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
    {2, EC_DIR_OUTPUT, 1, slave_motor_pdos + 0, EC_WD_ENABLE},
    {3, EC_DIR_INPUT, 1, slave_motor_pdos + 1, EC_WD_DISABLE},
    {0xff}};
/**********************CStruct**********************************/

// 电机回零
void homeing(struct _SlaveConfig *slave_config, struct _Domain *domain,
             int *motorMode, int motorID) {
  int8_t operation_mode_display =
      EC_READ_U16(domain->domain_pd +
                  motor_parm[motorID].operation_mode_display); // 读取状态字
  uint16_t status = EC_READ_U16(domain->domain_pd +
                                motor_parm[motorID].status_word); // 读取状态字
  if (operation_mode_display == MODEL_HOME && (status & 0xFF) == 0x37) {
    if ((status & 0xFF00) == 0x0600) {
      EC_WRITE_U16(domain->domain_pd + motor_parm[motorID].ctrl_word,
                   0x1F);                     // 启动运行
    } else if ((status & 0xFF00) == 0x1600) { // 到位
      printf("电机%d回零完成\n", motorID);
      *motorMode = MODEL_ENABLE;
      EC_WRITE_U16(domain->domain_pd + motor_parm[motorID].ctrl_word,
                   0x07); // 重置
      return;
    }
  } else {
    EC_WRITE_S8(domain->domain_pd + motor_parm[motorID].operation_mode,
                MODEL_HOME); // 设置操作模式

    EC_WRITE_U16(domain->domain_pd + motor_parm[motorID].ctrl_word,
                 0x06); // 电机得电
    EC_WRITE_U16(domain->domain_pd + motor_parm[motorID].ctrl_word,
                 0x07); // 电机得电
    EC_WRITE_U16(domain->domain_pd + motor_parm[motorID].ctrl_word,
                 0x0f); // 重置
    printf("电机%d开始回零运动\n", motorID);
  }
}

// 电机位置控制PP模式
void pp(struct _SlaveConfig *slave_config, struct _Domain *domain,
        int target_pos, int motorID, int *state) {
  int8_t operation_mode_display =
      EC_READ_U16(domain->domain_pd +
                  motor_parm[motorID].operation_mode_display); // 读取当前模式
  uint16_t status = EC_READ_U16(domain->domain_pd +
                                motor_parm[motorID].status_word); // 读取状态字
  static int last_pos = 0;
  static int pp_state = 0;
  if (*state < 10) {
    if (*state == 0) {
      EC_WRITE_U16(domain->domain_pd + motor_parm[motorID].ctrl_word,
                   0x06); // 重置电机
      if ((status & 0xFF) == 0x31) {
        *state = 1;
      }
    } else if (*state == 1) {
      EC_WRITE_S8(domain->domain_pd + motor_parm[motorID].operation_mode,
                  MODEL_PP); // 设置操作模式
      if (operation_mode_display == MODEL_PP) {
        *state = 2;
      }
    } else if (*state == 2) {
      EC_WRITE_U16(domain->domain_pd + motor_parm[motorID].ctrl_word,
                   0x07); // 电机得电
      if ((status & 0xFF) == 0x33) {
        *state = 3;
      }
    } else if (*state == 3) {
      EC_WRITE_U16(domain->domain_pd + motor_parm[motorID].ctrl_word,
                   0x2f); // 使能电机
      if ((status & 0xFF) == 0x37) {
        *state = 10;
        printf("state: %d\n", status);
      }
    }
  } else {
    if (last_pos != target_pos) {
      pp_state = 0;
    }
    if (pp_state < 10) {
      if (pp_state == 0) {
        EC_WRITE_S32(domain->domain_pd + motor_parm[motorID].target_pos,
                     target_pos); // 设置目标位置
        if (EC_READ_S32(domain->domain_pd + motor_parm[motorID].target_pos) ==
            target_pos) { //目标写入成功
          pp_state = 1;
        }
      } else if (pp_state == 1) {
        EC_WRITE_U16(domain->domain_pd + motor_parm[motorID].ctrl_word,
                     0x2f); // 使能电机
        if (EC_READ_U16(domain->domain_pd + motor_parm[motorID].ctrl_word) ==
            0x2f) { // 电机使能成功
          pp_state = 2;
          printf("电机状态字1：%d\n", status);
        }
      } else if (pp_state == 2) {
        EC_WRITE_U16(domain->domain_pd + motor_parm[motorID].ctrl_word,
                     0x3f); // 使能电机
        if (EC_READ_U16(domain->domain_pd + motor_parm[motorID].ctrl_word) ==
            0x3f) { // 电机使能成功
          pp_state = 10;
          printf("电机状态字2：%d\n", status);
        }
      }
    }
    last_pos = target_pos;
  }
}

// 电机位置控制CSP模式
void csp(struct _SlaveConfig *slave_config, struct _Domain *domain,
         int target_pos, int motorID, int *state) {
  int8_t operation_mode_display =
      EC_READ_U16(domain->domain_pd +
                  motor_parm[motorID].operation_mode_display); // 读取状态字
  uint16_t status = EC_READ_U16(domain->domain_pd +
                                motor_parm[motorID].status_word); // 读取状态字

  if (*state < 10) {
    if (*state == 0) {
      EC_WRITE_U16(domain->domain_pd + motor_parm[motorID].ctrl_word,
                   0x06); // 重置电机
      if ((status & 0xFF) == 0x31 || (status & 0xFF) == 0xb1) {
        *state = 1;
      }
    } else if (*state == 1) {
      EC_WRITE_S8(domain->domain_pd + motor_parm[motorID].operation_mode,
                  MODEL_CSP); // 设置操作模式
      if (operation_mode_display == MODEL_CSP) {
        *state = 2;
      }
    } else if (*state == 2) {
      int actual_motor =
          EC_READ_S32(domain->domain_pd + motor_parm[motorID].current_pos);
      int target_motor =
          EC_READ_S32(domain->domain_pd + motor_parm[motorID].target_pos);
      EC_WRITE_S32(domain->domain_pd + motor_parm[motorID].target_pos,
                   actual_motor); // 设置目标位置
      int diff1 = target_motor - actual_motor;
      int diff2 = target_pos - actual_motor;
      if (-100 < diff1 && diff1 < 100 && -1000 < diff2 && diff2 < 1000) {
        *state = 3;
      }
    } else if (*state == 3) {
      EC_WRITE_U16(domain->domain_pd + motor_parm[motorID].ctrl_word,
                   0x07); // 电机得电
      if ((status & 0xFF) == 0x33 || (status & 0xFF) == 0xb3) {
        *state = 4;
      }
    } else if (*state == 4) {
      EC_WRITE_U16(domain->domain_pd + motor_parm[motorID].ctrl_word,
                   0x0f); // 使能电机
      if ((status & 0xFF) == 0x37 || (status & 0xFF) == 0xb7) {
        *state = 10;
      }
    }
  } else {
    EC_WRITE_S32(domain->domain_pd + motor_parm[motorID].target_pos,
                 target_pos); // 设置目标位置
    printf("电机%d开始位置控制\n", motorID);
  }
}

void enable(struct _SlaveConfig *slave_config, struct _Domain *domain,
            int motorID, int *state) {
  uint16_t status = EC_READ_U16(domain->domain_pd +
                                motor_parm[motorID].status_word); // 读取状态字

  if (*state < 10) {
    if (*state == 0) {
      EC_WRITE_U16(domain->domain_pd + motor_parm[motorID].ctrl_word,
                   0x06); // 重置电机
      if ((status & 0xFF) == 0x31 || (status & 0xFF) == 0xb1) {
        *state = 1;
      }
    } else if (*state == 1) {
      EC_WRITE_U16(domain->domain_pd + motor_parm[motorID].ctrl_word,
                   0x07); // 电机得电
      if ((status & 0xFF) == 0x33 || (status & 0xFF) == 0xb3) {
        *state = 2;
      }
    } else if (*state == 2) {
      EC_WRITE_U16(domain->domain_pd + motor_parm[motorID].ctrl_word,
                   0x0f); // 使能电机
      if ((status & 0xFF) == 0x37 || (status & 0xFF) == 0xb7) {
        *state = 10;
      }
    }
  }
}

void disable(struct _SlaveConfig *slave_config, struct _Domain *domain,
             int motorID, int *state) {
  uint16_t status = EC_READ_U16(domain->domain_pd +
                                motor_parm[motorID].status_word); // 读取状态字

  if (*state < 10) {
    if (*state == 0) {
      EC_WRITE_U16(domain->domain_pd + motor_parm[motorID].ctrl_word,
                   0x06); // 重置电机
      if ((status & 0xFF) == 0x31 || (status & 0xFF) == 0xb1) {
        *state = 10;
      }
    }
  }
}

bool initCDHD(struct _Domain *domain) {
  const int cdhd_id = 0;
  static int cdhd_state = 0;
  if (cdhd_state == 10) {
    return true;
  }
  uint16_t cdhd_status =
      EC_READ_U16(domain->domain_pd + motor_parm[cdhd_id].status_word);

  if (cdhd_state == 0) {
    EC_WRITE_U16(domain->domain_pd + motor_parm[cdhd_id].ctrl_word,
                 0x06); // 重置电机
    if ((cdhd_status & 0xFF) == 0xb1) {
      cdhd_state = 1;
    }
  } else if (cdhd_state == 1) {
    EC_WRITE_U16(domain->domain_pd + motor_parm[cdhd_id].ctrl_word,
                 0x07); // 电机得电
    if ((cdhd_status & 0xFF) == 0xb3) {
      cdhd_state = 2;
    }
  } else if (cdhd_state == 2) {
    EC_WRITE_U16(domain->domain_pd + motor_parm[cdhd_id].ctrl_word,
                 0x0f); // 使能电机
    if ((cdhd_status & 0xFF) == 0xb7) {
      cdhd_state = 3;
    }
  } else if (cdhd_state == 3) {
    EC_WRITE_U16(domain->domain_pd + motor_parm[cdhd_id].ctrl_word,
                 0x06); // 使能电机
    if ((cdhd_status & 0xFF) == 0x31) {
      cdhd_state = 10;
      printf("cdhd 0号电机初始化完成\n");
    }
  }
  return false;
}

// 主电机控制函数
void motor_main(struct _SlaveConfig *slave_config, struct _Domain *domain) {
  pthread_mutex_lock(&cmdSet_mutex);
  updateStatues(&cmdSet);

  cmdSet.actual_pos[0] =
      EC_READ_S32(domain->domain_pd + motor_parm[0].current_pos);

  /************************** 初始化电机 **************************/
  static int init_flag = 0;

  // --- 核心修改：故障恢复逻辑 ---
  uint16_t status = EC_READ_U16(domain->domain_pd + motor_parm[0].status_word);

  // 检查状态字中的 Fault 位 (bit 3)
  if ((status & 0x0008) != 0) {
    printf("检测到驱动器故障 (Status=0x%04X)，正在尝试复位...\n", status);
    // 通过写入控制字的 Fault Reset 位 (bit 7) 来清除故障
    EC_WRITE_U16(domain->domain_pd + motor_parm[0].ctrl_word, 0x80);

    // 由于正在进行故障复位，本次循环不执行其他任何操作
    init_flag = 0;               // 将初始化状态机重置
    g_robotStateData.status = 0; // 保持在初始化/运行中状态
    pthread_mutex_unlock(&cmdSet_mutex);
    return; // 直接返回，等待下一个周期检查复位是否成功
  }

  // printf("初始化%d状态,电机%d状态\n", init_flag, cmdSet.state[0]);

  if (init_flag < 10) {
    if (init_flag == 0) {
      int8_t operation_mode_display = 0;
      bool motor_start_flag = true;
      operation_mode_display =
          EC_READ_S8(domain->domain_pd +
                     motor_parm[0].operation_mode_display); // 读取状态字

      if (operation_mode_display != 1 && operation_mode_display != 8) {
        motor_start_flag = false;
      }
      if (motor_start_flag) {
        init_flag = 1;
      }
    }
    if (init_flag == 1) { // 设置目标位置
      if (cmdSet.actual_pos[0] < -1500000 || cmdSet.actual_pos[0] > 690000) {
        // 范围错误：设置状态为104
        g_robotStateData.status = 104; // 范围错误
        printf("错误：电机位置超出范围 (%d)，status=104\n",
               cmdSet.actual_pos[0]);
      }

      cmdSet.target_pos[0] = cmdSet.actual_pos[0];
      init_flag = 2;
    } else if (init_flag == 2) { // 执行第一次初始化
      for (int i = 0; i < MOTOR_NUM; i++) {
        pp(slave_config, domain, cmdSet.target_pos[i], i, &cmdSet.state[i]);
      }
      if (cmdSet.state[0] == 10) {
        init_flag = 10;
        g_robotStateData.status = 1; // 初始化完成,可以接收任务
        cmdSet.target_pos[0] = cmdSet.actual_pos[0];
        printf("电机初始化完成，可以接收任务\n");
      }
    }
  }

  // 执行命令集
  for (int i = 0; i < MOTOR_NUM; i++) {
    pp(slave_config, domain, cmdSet.target_pos[i], i, &cmdSet.state[i]);
    g_robotStateData.parameter = cmdSet.actual_pos[i] / AXIS1_K; // 更新位置数据
  }

  pthread_mutex_unlock(&cmdSet_mutex);
}